Generally when controlling an articulated arm using a hand control or any other type of control a command is fed to the control computer directing that the arm be moved in a particular manner i.e. a particular distance at a particular velocity in a particular direction. Obviously these commands are to cause a selected end point on the arm to move toward a selected target. Thus normally the input to the control is to move the end point of the arm a specified distance (in a selected time period) in a selected direction. These instructions must be modified as the arm is moved by adjusting the angles at each of the joints and it is necessary to determine the joint angles for each of the articulated joints that are to be adjusted to obtain the movement.
The term jointspace is normally used to define the range of angular positions of each joint and workspace defines the range of positions of the selected end point of the arm in space.
The transformation from jointspace to workspace is simple. A set of closed-form equations can be easily derived for the forward kinematics transformations. EQU p=f(.theta.) (1)
Where
p=workspace vector PA1 .theta.=jointspace vector PA1 K=square root of the damping factor k PA1 k.sub.3 =the adjusted damping factor PA1 k=a selected constant in the range of (0.001 to 1) times the total length of the articulated arm in meters. PA1 .parallel..delta.p.parallel.=the distance between the end point and target after each iteration PA1 x=is a preselected number between 1 and 3. PA1 I=the identity matrix PA1 k.sub.3 =a variable damping factor determined by the equation EQU k.sub.3 =k.parallel..delta.p.parallel..sup.x ( 12) PA1 where:
A general method for deriving the forward kinematic transformation is well known. An accurate transformation is required from workspace to jointspace, the inverse kinematics transformation, is required for control of the manipulator or articulated arm. EQU .theta.=f.sup.-1 (p) (2)
Simple closed-form inverse kinematics equations are possible only for a limited number of arm designs and are accurate only away from a singularity i.e. in a high manipulability region of the workspace. Singularities or singular points are boundaries between alternative joint solutions for a given target position and generally, adjacent to a singularity there are several solutions close together.
In substantially all applications the conversion from workspace to jointspace must be made efficiently to permit real time operation of the controlled arm or manipulator.
One well known method of solving inverse kinematics problems is to use an iterative method based on the Jacobian matrix. An iteration scheme can be applied to estimate the jointspace values required to position the manipulator at a point in the workspace (target). In these solutions important considerations are the starting jointspace values and the direction and magnitude of the joint corrections.
The Jacobian is a liner relationship between different changes in jointspace and different changes in workspace EQU J(.theta.).delta..theta.=.delta.p (3) EQU where: EQU J(.theta.)=.delta.f(.theta.)/.delta..theta. (4)
General methods are known for calculating the Jacobian matrix for an arbitrary manipulator and this Jacobian matrix is inverted to convert differential changes in workspace to changes in jointspace.
The set of equations (3) can also be solved using Gaussian elimination to obtain the joint corrections.
The Jacobian usually gives a good estimate of joint corrections required to minimize errors in the workspace, however difficulties areise when the manipulator is near a singularity or when the Jacobian matrix is not square.
The inversion of a Jacobian matrix is not possible at a singular point as the determinant of the Jacobian is zero at this point. Near singularities small changes in the workspace require physically large joint changes and the joint corrections determined by the inverse Jacobian become extremely large and incorporate large errors. In other words the use of Jacobian algorithms near singular points results in significant instability of the arm.
Attempts to overcome this problem have involved the use of the pseudoinverse of the Jacobian matrix. The pseudoinverse is particularly useful where the Jacobian matrix is not square. A general pseudoinverse is given by the formula: EQU J.sup.+ (.theta.)=(J.sup.T (.theta.)J(.theta.)).sup.-1 J.sup.T (.theta.) (5)
In the case where the rank of the Jacobian is less than the number of degrees of freedom of the end effector in the particular arm being manipulated the solution of equation (3) minimizes the error in the solution in the least squares sense. EQU .parallel.J(.theta.).delta..theta.-.delta.p.parallel. (6)
The pseudoinverse does not have to be derived explicitly. The set of equations given by: EQU (J.sup.T (.theta.)J(.theta.)).delta..theta.=J.sup.T (.theta.) .delta.p (7)
can be solved for .delta.O with fewer operations using Gaussian elimination.
If the Jacobian matrix is symmetric the number of operations required to calculate may be reduced for example an n X n Jacobian using one method (Wampler's described below) J.sup.T (.theta.)J(.theta.) requires (n.sup.3 +n.sup.2)/2 multiplications and (n.sup.3 -n.sup.2)/2 additions. The multiplication of the position errors by the transpose of the Jacobian requires n.sup.2 multiplications and (n.sup.2 -n) additions. In the case of a 6 degree of freedom manipulator about 162 manipulations and 120 additions are required to set up the equation.
For redundant manipulators it has been suggested to use an alternative formulation with a reduced number of calculations for the pseudoinverse, J.sup.+ (.theta.) of the matrix J(.theta.). EQU J.sup.+ (.theta.)=J.sup.T (.theta.)(J(.theta.)J.sup.T (.theta.)).sup.-1 ( 8)
As above indicated the pseudoinverse does not exist if the determinate of J(O)J.sup.T (.theta.) is zero which renders the system ineffective at singular points.
A number of algorithms using pseudoinverse techniques for redundant and non redundant manipulators have been tested to solve the inverse kinematics of multi-jointed manipulators (up to about 7 joints), however these solutions did not suggest how to handle problems near or at a singularity.
It has also been suggested to iteratively vary the magnitude of the joint correction obtained using the pseudoinverse if the joint correction did not move the manipulator closer to the target and using another level of iteration to solve for the correct magnitude of joint correction. However these solutions are not sufficiently efficient for real time implementation and furthermore the assumption that the direction is correct may not be valid for a singularity since the Jacobian changes greatly with small changes in jointspace.
Another teaching as described by C. W. Wampler II in "Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least Squares Method", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-16, No. 1, January/February 1986, p 93-101 stabilizes the jointspace calculations near singular points by damping the differential joint changes as indicated by the following: ##EQU1## where: I=identity matrix
The pseudoinverse of this over defined set of equations has a constant factor k added to the diagonal elements of the square product matrix. EQU J.sup.+ (.theta.)=(J.sup.T (.theta.)J(.theta.)+kI).sup.-1 J.sup.T (.theta.) (10)
This modification of the pseudoinverse results in a positive defined matrix that is guaranteed to be invertible.
Wampler also suggested that a fixed damping factor be used and that this damping constant be chosen to insure stability at or adjacent to singular points and for out of reach targets. This damping poses limits to the size of the errors as well as the size of the solution. Damping is equivalent to minimizing the following function. EQU .parallel.J(.theta.).delta..theta.-.delta.p.parallel..sup.2 +k.parallel..delta..theta..parallel..sup.2 ( 11)
Near singularities the damping factor causes the singular value of the pseudoinverse to go towards zero instead of toward infinity. With damping the joint corrections are kept to finite values, however, there is a compromise between stability and the error in solving for the joint corrections in equations (3). For stability the damping value is chosen for operation of the manipulator near a singular point, however at a distance away from the singular point this damping factor is not effective.
Wampler chose a large damping factor to give stability by driving the joint corrections to zero for out of reach targets, however with a large damping constant the manipulator is unable to reach positions close to the singular points, joint corrections go to zero too quickly (for target positions which are close to singular points) for the manipulator to reach those targets. The fixed damping constant is also inefficient for targets near singular points because the joint corrections are very small and convergence to the solution is very slow.
It has been proposed to overcome this problem by the use of a variable damping technique which provides a compromise between the size of the error in equation (6) and obtaining feasible joint corrections (see Y. Nakamura, "Kinematic Studies on the Trajectory Control of Robotic Manipulators", Ph.D thesis Automation Research Laboratory, Kyoto University, June 1986.) This modified pseudoinverse is described in the art as a singular robust inverse or SR-inverse. At positions far away from singularities no damping is needed, as the manipulator nears a singular point more damping is applied. The manipulability of the manipulator is calculated to determine it's closeness to a singular point. The amount of computation required to determine nearness to a singular point can be very large. The method using estimation of the manipulability would require in the order of n.sup.3 operations where n is the number of degrees of freedom of the manipulator.
As with the fixed damping method the SR-inverse has relatively large damping values close to singular points, thus motion near singular points would be restricted.
Near singularities in the error in the SR techniques is still very large. With the variable damping scheme the stability for unreachable positions which is possible using a fixed damping technique is lost if a low damping value is used as is normally the case in the high manipulability region. The use of the variable damping factor is to insure the pseudoinverse is invertible, yet not inhibit operation away from singular points. Accuracy is not possible for positions near singular points.